#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>


#include<cv_bridge/cv_bridge.h>
// #include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <image_transport/image_transport.h>


#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>
#include <map>
#include <pcl/filters/passthrough.h>
#include <chrono>
#include <thread>
#include"visual_odom/config.h"
#include"visual_odom/frame.h"
#include"visual_odom/frontend.h"
#include"visual_odom/camera.h"
#include"visual_odom/show_pub.h"

test_visual_odom::Config::Ptr p_config_;
test_visual_odom::Frontend::Ptr p_frontend_; 
test_visual_odom::ShowPub::Ptr p_show_pub_;

void img_CB(const sensor_msgs::ImageConstPtr& left_img_msg, const sensor_msgs::ImageConstPtr& right_img_msg){
    test_visual_odom::Frame::Ptr p_current_frame;
    p_current_frame.reset(new test_visual_odom::Frame(cv_bridge::toCvCopy(left_img_msg, sensor_msgs::image_encodings::BGR8)->image, 
        cv_bridge::toCvCopy(right_img_msg, sensor_msgs::image_encodings::BGR8)->image));
    p_frontend_->addFrame(p_current_frame);
    ROS_DEBUG("cb3");
}

int main(int argc, char* argv[])
{
    if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,ros::console::levels::Debug))
    {
        ros::console::notifyLoggerLevelsChanged();
    }

    ros::init(argc, argv, "det_handle_img");
    ros::NodeHandle nh;

    p_config_.reset(new test_visual_odom::Config(nh, "/visual_lidar_slam/config_file"));
    test_visual_odom::Camera::Ptr p_camera_left,p_camera_right;
    Eigen::Matrix<double, 3, 1> t;
    t<<0,0,0;
    p_camera_left.reset(new test_visual_odom::Camera(p_config_->camera_left_fx_, p_config_->camera_left_fy_, p_config_->camera_left_cx_, p_config_->camera_left_cy_,
                                                SE3(SO3(), t)));
    Eigen::Matrix3d R_camera_l2r;
    cv::cv2eigen(p_config_->R_camera_left2camera_right_, R_camera_l2r);
    cv::cv2eigen(p_config_->t_camera_left2camera_right_,t);
    p_camera_right.reset(new test_visual_odom::Camera(p_config_->camera_right_fx_, p_config_->camera_right_fy_, p_config_->camera_right_cx_, p_config_->camera_right_cy_,
                                                SE3(SO3(R_camera_l2r), t)));

    p_frontend_.reset(new test_visual_odom::Frontend(p_config_, p_camera_left, p_camera_right));

    p_show_pub_.reset(new test_visual_odom::ShowPub(nh));
    p_frontend_->setShowPub(p_show_pub_);

    message_filters::Subscriber<sensor_msgs::Image> left_image_sub(nh, p_config_->left_image_topic_, 5);
    message_filters::Subscriber<sensor_msgs::Image> right_image_sub(nh, p_config_->right_image_topic_, 5);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> MySyncPolicy;
    // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(20), left_image_sub, right_image_sub);
    sync.setMaxIntervalDuration(ros::Duration(0.05));
    sync.registerCallback(boost::bind(&img_CB, _1, _2));

    ros::spin();

    return 0;
}
